import java.util.ArrayList;

import processing.core.PApplet;
import processing.core.PVector;
import remixlab.proscene.Scene;
import SimpleOpenNI.SimpleOpenNI;
import codeanticode.glgraphics.GLGraphicsOffScreen;
import codeanticode.glgraphics.GLTexture;
import deadpixel.keystone.Keystone;
import edu.maping.easy.painter.ThingPainterDelegate;
import edu.maping.easy.painter.napplet.EMNAppletAdapter;

public abstract class OpenNiEMWrapper extends EMNAppletAdapter {

	protected SimpleOpenNI context;
	float zoomF = 0.5f;
	float rotX = radians(180); // by default rotate the hole scene 180deg around
								// the x-axis,
								// the data from openni comes upside down
	float rotY = radians(0);
	int[] userColors = { color(255, 0, 0), color(0, 255, 0), color(0, 0, 255),
			color(255, 255, 0), color(255, 0, 255), color(0, 255, 255) };
	int[] userCoMColors = { color(255, 100, 100), color(100, 255, 100),
			color(100, 100, 255), color(255, 255, 100), color(255, 100, 255),
			color(100, 255, 255) };

	// GLGraphicsOffScreen glg1 = null;
	protected GLGraphicsOffScreen glg2 = null;

	protected Scene scene = null;

	protected boolean calibrating = true;

	protected boolean calibratingKeystone = true;

	boolean withPoints = true;

	boolean kinectMode = false;

	public void setup() {

		// if i use P3D, in opengl there is no problem
		PApplet pApplet = this;
		if (parentPApplet != null)
			pApplet = parentPApplet;

		// glg1 = new GLGraphicsOffScreen(pApplet, (int) (width), (int)
		// (height),
		// true, 4);
		// glg2 = new GLGraphicsOffScreen(pApplet, width, height, false, 4);
		glg2 = new GLGraphicsOffScreen(pApplet, width, height, true, 4);//

		ks = new Keystone(this);
		surface = ks.createCornerPinSurface(width, height, 20);

		ks.toggleCalibration();

		scene = new Scene(this, glg2);
		scene.setGridIsDrawn(true);
		scene.setAxisIsDrawn(true);
		scene.setRadius(2000);
		scene.showAll();
		scene.disableBackgroundHanddling();

		scene.disableKeyboardHandling();
		scene.disableMouseHandling();

		if (kinectMode) {
			context = new SimpleOpenNI(this,
					SimpleOpenNI.RUN_MODE_MULTI_THREADED);

			// disable mirror
			context.setMirror(false);

			// enable depthMap generation
			context.enableDepth();

			// enable skeleton generation for all joints
			context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

			// enable the scene, to get the floor
			context.enableScene();
		}

	}

	ArrayList<PVector> users = new ArrayList<PVector>();
	ArrayList<PVector> realUsers = new ArrayList<PVector>();
	PVector realUser = null;

	public void update() {
		// update the cam
		if (kinectMode)
			context.update();

		if (calibrating) {
			glg2.beginDraw();
			scene.beginDraw();
		}

		if (kinectMode) {
			users = do3d();
			glg2.popStyle();
		}

		// draw the kinect cam
		if (calibrating && kinectMode)
			context.drawCamFrustum();

		postDraw();

	}

	public void postDraw() {
		// image(glg1.getTexture(), 10, 10, width, height / 2);
		if (calibrating) {

			scene.endDraw();
			glg2.endDraw();
		}
		if (calibrating) {
			// tint(255, 255);
			// image(glg2.getTexture(), 10, 10, width, height);

			GLTexture glTexture = glg2.getTexture();
			surface.render(glTexture);
		}
		// else
		// image(glg1.getTexture(), 10, 10, width, height);
	}

	protected ArrayList<PVector> do3d() {
		int[] depthMap = context.depthMap();
		int steps = 3; // to speed up the drawing, draw every third point
		int index;
		PVector realWorldPoint;

		// translate(0, 0, -1000); // set the rotation center of the scene
		// 1000
		// infront of the camera

		int userCount = context.getNumberOfUsers();

		if (withPoints) {
			int[] userMap = null;
			if (userCount > 0) {
				userMap = context.getUsersPixels(SimpleOpenNI.USERS_ALL);
			}

			for (int y = 0; y < context.depthHeight(); y += steps) {
				for (int x = 0; x < context.depthWidth(); x += steps) {
					index = x + y * context.depthWidth();
					if (depthMap[index] > 0) {
						// get the realworld points
						realWorldPoint = context.depthMapRealWorld()[index];

						// check if there is a user
						if (userMap != null && userMap[index] != 0) { // calc
							// the
							// user
							// color
							int colorIndex = userMap[index] % userColors.length;
							glg2.stroke(userColors[colorIndex]);
						} else
							// default color
							glg2.stroke(100);
						glg2.point(realWorldPoint.x, realWorldPoint.y,
								realWorldPoint.z);
					}
				}
			}
		}

		float x = -1;
		float y = -1;
		float z = -1;
		// draw the center of mass
		PVector pos = new PVector();
		glg2.pushStyle();
		glg2.strokeWeight(15);

		ArrayList<PVector> users = new ArrayList<PVector>();
		realUsers = new ArrayList<PVector>();

		realUser = null;

		for (int userId = 1; userId <= 4; userId++) {
			context.getCoM(userId, pos);

			if (pos.z > 0) {
				realUser = pos.get();
				realUsers.add(pos.get());
				glg2.stroke(userCoMColors[userId % userCoMColors.length]);
				// glg2.point(pos.x, pos.y, pos.z);
				x = glg2.screenX(pos.x, pos.y, pos.z);
				y = glg2.screenY(pos.x, pos.y, pos.z);
				z = glg2.screenZ(pos.x, pos.y, pos.z);
				PVector vector = new PVector(x, y, z);
				users.add(vector);
			}
		}

		return users;
	}

	@Override
	public void keyPressed() {
		switch (key) {
		case 'v':
			calibrating = !calibrating;
			break;
		case 'ñ':
			withPoints = !withPoints;
			break;
		case 'c':
			if (calibratingKeystone) {
				ks.stopCalibration();
				scene.enableKeyboardHandling();
				scene.enableMouseHandling();
				calibratingKeystone = !calibratingKeystone;
			} else {
				ks.startCalibration();
				scene.disableKeyboardHandling();
				scene.disableMouseHandling();
				calibratingKeystone = !calibratingKeystone;
			}
			break;

		default:
			break;
		}
	}

	// -----------------------------------------------------------------
	// SimpleOpenNI user events

	public void onNewUser(int userId) {
		println("onNewUser - userId: " + userId);
	}

	public void onLostUser(int userId) {
		println("onLostUser - userId: " + userId);
	}
}
